#include "CtpForwardingEngine.h"
#include "pr.h"
#include "Ctp.h"
generic module CtpForwardingEngineP() {
	provides interface Init;
	provides interface StdControl;
	provides interface Send[uint8_t client];
	provides interface Receive[collection_id_t id];
	provides interface Receive as Snoop[collection_id_t id];
	provides interface Intercept[collection_id_t id];
	provides interface Packet;
	provides interface CollectionPacket;
	provides interface CtpPacket;
	provides interface CtpCongestion;
	//provides interface CtpFwInfo;
	
	
	
	
	uses interface AMSend as SubSend;
	uses interface PacketAcknowledgements;
	uses interface Timer<TMilli> as RetxmitTimer;
	uses interface LinkEstimator;
	uses interface UnicastNameFreeRouting;
	uses interface Packet as SubPacket;
	
	
	uses interface Queue<fe_queue_entry_t*> as SendQueue;
    uses interface Pool<fe_queue_entry_t> as QEntryPool;
    uses interface Pool<message_t> as MessagePool;
    uses interface Cache<message_t*> as SentCache;
	
	
	uses interface Receive as SubReceive;
	uses interface Receive as SubSnoop;
	uses interface SplitControl as RadioControl;
	uses interface CtpInfo;
	uses interface RootControl;
	uses interface CollectionId[uint8_t client];
	uses interface AMPacket;
	uses interface Random;
	//uses interface MyLeds as Leds;
	uses interface CollectionDebug;
}

implementation {
	
	
	enum{
		QUEUE_CONGESTED = 0x1,
		ROUTING_ON		= 0x2,
		RADIO_ON 		= 0x4,
		ACK_PENDING 	= 0x8,
		SENDING 		= 0x10
	};
	
	enum{
		CLIENT_COUNT = uniqueCount(UQ_CTP_CLIENT)
	};
	
	uint8_t forwardingState = 0;
	uint8_t retriesOver30 = 0;
	am_addr_t  lastParent;
	
	uint8_t seqno;
	message_t loopbackMsg;
	//message_t* msg;
	message_t* ONE_NOK loopbackMsgPtr;
	fe_queue_entry_t clientEnrties[CLIENT_COUNT];
	fe_queue_entry_t* ONE_NOK clientPtrs[CLIENT_COUNT];
	
	void clearState(uint8_t state) {
		forwardingState = forwardingState & ~state;
	}
	
	bool hasState(uint8_t state) {
		return forwardingState & state;
	}
	
	void setState(uint8_t state) {
		forwardingState = forwardingState | state;
	}
	
	
	command error_t Init.init(){
		uint8_t i;   // why not uint8_t?
		for(i = 0; i < CLIENT_COUNT;i++){
			clientPtrs[i] = clientEnrties + i; //?
			//pr("%d\n",clientPtrs[i]);
		}
		loopbackMsgPtr = &loopbackMsg;
		seqno = 0;
		//pr("Init");
		return SUCCESS;
	}
	
	command error_t StdControl.start(){
		setState(ROUTING_ON);
		return SUCCESS;
	}
	
	command error_t StdControl.stop(){
		clearState(ROUTING_ON);
		return SUCCESS;
	}
	
	task void sendTask();
	
	event void RadioControl.startDone(error_t err){
		if(err == SUCCESS) {
			setState(RADIO_ON);
			if(!call SendQueue.empty()) {
				post sendTask();
			}
		}
	}
	
	event void RadioControl.stopDone(error_t err) {
		if(err == SUCCESS) {
			clearState(RADIO_ON);
		}
	}
	
	static void startRetxmitTimer(uint16_t window, uint16_t offset) {
		uint16_t r = call Random.rand16();
		r %= window;
		r += offset;
		call RetxmitTimer.startOneShot(r);
	}
	
	
	event void UnicastNameFreeRouting.routeFound() {
		post sendTask();
	}
	
	
	event void UnicastNameFreeRouting.noRoute() {
		
		
	}
	
	ctp_data_header_t* getHeader(message_t* m) {
		//pr("getHeader\n");
		return (ctp_data_header_t*)call SubPacket.getPayload(m,sizeof(ctp_data_header_t));
	}
	
	command error_t Send.send[uint8_t client](message_t* msg, uint8_t len) {
		ctp_data_header_t* hdr;
		//ctp_data_header_t* hdr2;
		fe_queue_entry_t *qe;
		pr("Start Send\n");
		if(!hasState(ROUTING_ON)) {
			//pr("!hasState(ROUTING_ON)\n");
			return EOFF;
		}
		if(len > call Send.maxPayloadLength[client]()) {
			//pr("lend > maxPayloadLength\n");
			return ESIZE;
		}
		call Packet.setPayloadLength(msg,len);
		hdr = getHeader(msg);
		hdr->options = 0xAA;
		hdr->thl = 0;
		hdr->origin = TOS_NODE_ID;
		//hdr->origin = 0xABCD;
		hdr->originSeqNo = seqno++;
		//hdr->originSeqNo = 0xBB;
		hdr->type = call CollectionId.fetch[client]();
		/*hdr2 = getHeader(msg);
		 *(pr("options = %d\t",hdr2->options);
		 *pr("origin = %d\t",hdr2->origin);
		 *pr("originSeqNo = %d\t",hdr2->originSeqNo);
		 *pr("type = %d\t",hdr2->type);
		 *pr("thl = %d\n",hdr2->thl);
		 */
		
		
		if(clientPtrs[client] == NULL) { // null?
			pr("clientPtrs[client] == NULL\n");
			return EBUSY;
		}
		
		qe = clientPtrs[client];
		//pr("%s\n",msg->header);
		qe->msg = msg;
		qe->client = client;
		qe->retries = MAX_RETRIES;
		//pr("MAX_RETRIES\n");
		if(call SendQueue.enqueue(qe) == SUCCESS){
			pr("SendQueue.enqueue(qe) =SUCCESS\n");
			if(hasState(RADIO_ON) && !hasState(SENDING)) {
				post sendTask();
			}
			clientPtrs[client] = NULL;
			return SUCCESS;
		}
		else {
			//pr("SendQueue.enqueue(qe) =FAIL)\n");
			return FAIL;  // could not be enqueued
		}
	}
	
	
	
	task void sendTask() {
		pr("sendTask Start\n");
		if(hasState(SENDING) || call SendQueue.empty()) {
			//pr("Task_1\n");
			return;
		}
		else if(!call RootControl.isRoot() && !call UnicastNameFreeRouting.hasRoute()) {
			//pr("Task_2\n");
			call RetxmitTimer.startOneShot(NO_ROUTE_RETRY);
			return;
		}
		else {
			error_t subsendResult;
			fe_queue_entry_t* qe = call SendQueue.head();
			uint8_t payloadLen = call SubPacket.payloadLength(qe->msg);
			am_addr_t dest = call UnicastNameFreeRouting.nextHop();
			pr("parent = %d\n",dest);
			if(call SentCache.lookup(qe->msg)){
				call SendQueue.dequeue();
				call MessagePool.put(qe->msg);
				call QEntryPool.put(qe);
				post sendTask();
				return;
			}
			
			
			if(call RootControl.isRoot()) {
				collection_id_t collectid = getHeader(qe->msg)->type;
				uint8_t* payload;
				uint8_t payloadLength;
				pr("sendTask is Root\n");
				memcpy(loopbackMsgPtr,qe->msg, sizeof(message_t));
				
				payloadLength = call Packet.payloadLength(loopbackMsgPtr);
				payload = call Packet.getPayload(loopbackMsgPtr, payloadLength);
				loopbackMsgPtr = signal Receive.receive[collectid](loopbackMsgPtr,payload,payloadLength);
				signal SubSend.sendDone(qe->msg,SUCCESS);
			}
			else {
				//call CtpPacket.clearOption(qe->msg,CTP_OPT_ECN | CTP_OPT_PULL);
				
				if(call PacketAcknowledgements.requestAck(qe->msg) == SUCCESS) {
					setState(ACK_PENDING);
				}
				if(hasState(QUEUE_CONGESTED)) {
					//call CtpPacket.setOption(qe->msg,CTP_OPT_ECN);
					if(call MessagePool.size() >=3 && call QEntryPool.size() >=3) {
						clearState(QUEUE_CONGESTED);
					}
				}
				subsendResult = call SubSend.send(dest,qe->msg,payloadLen);
				
				if(subsendResult == SUCCESS) {
					setState(SENDING);
				}
				else if(subsendResult == ESIZE) {
					call Packet.setPayloadLength(qe->msg,call Packet.maxPayloadLength());
					post sendTask();
				}
				else {
					// subsend failed
				}
				
			}
		}	
	}
	
	void packetComplete(fe_queue_entry_t* qe, message_t* msg, bool success) {
		if(qe->client < CLIENT_COUNT){  // node own packet
			clientPtrs[qe->client] = qe;
			signal Send.sendDone[qe->client](msg,SUCCESS); // ??    if success equal FAIL , Send.sendDone's error also equal SUCCESS ??
		}
		else {  // transmit packet
			if(success){
				call SentCache.insert(qe->msg);
			}
			else {
				
			}
			call MessagePool.put(qe->msg);
			call QEntryPool.put(qe);
		}
	}
	
	

	command error_t Send.cancel[uint8_t client](message_t* msg) {
		return FAIL;
	}
	
	command uint8_t Send.maxPayloadLength[uint8_t client](){
		return call Packet.maxPayloadLength();
	}
	
	command void* Send.getPayload[uint8_t client](message_t* msg,uint8_t len) {
		return call Packet.getPayload(msg,len);
	}
	
	event void SubSend.sendDone(message_t* msg, error_t error) {
		fe_queue_entry_t* qe = call SendQueue.head();
		if(error != SUCCESS) {
			startRetxmitTimer(SENDDONE_FAIL_WINDOW, SENDDONE_FAIL_OFFSET);
		}
		else if(hasState(ACK_PENDING) && !call PacketAcknowledgements.wasAcked(msg)) {
			//call linkEstimator.txNoAck(call AMPacket.destination(msg));
			//call CtpInfo.recomputeRoutes();
			//call CtpInfo.needBeacon();
			if(--qe->retries) {
				pr("retries transmit\n");
				startRetxmitTimer(SENDDONE_NOACK_WINDOW, SENDDONE_NOACK_OFFSET);
				//signal CtpFwInfo.ctpRetrans(msg, 
											//call Packet.getPayload(msg,call Packet.payloadLength(msg)),
											//call Packet.payloadLength(msg));
			}
			else {
				pr("retries > 30\n");
				retriesOver30++;
				if(retriesOver30 > 5) {
					call CtpInfo.parentNeedChange();
					retriesOver30 = 0;
				}

				call SendQueue.dequeue();
				clearState(SENDING);
				startRetxmitTimer(SENDDONE_OK_WINDOW, SENDDONE_OK_OFFSET);
				packetComplete(qe,msg,FALSE);
			}
		}
		else {
			call SendQueue.dequeue();
			clearState(SENDING);
			startRetxmitTimer(SENDDONE_OK_WINDOW, SENDDONE_OK_OFFSET);
			//call LinkEstimator.txAck(call AMPacket.destination(msg));
			packetComplete(qe,msg,TRUE);
		}
	}
	
	
	message_t* ONE forward(message_t* ONE m) {
		if(call MessagePool.empty()) {
			
		}
		else if(call QEntryPool.empty()) {
			
		}
		else {
			message_t* newMsg;
			fe_queue_entry_t* qe;
			
			qe = call QEntryPool.get();
			if(qe == NULL){
				return m;
			}
			
			newMsg = call MessagePool.get();
			if(newMsg == NULL) {
				return m;
			}
			
			if(call MessagePool.empty() || call QEntryPool.empty()) {
				setState(QUEUE_CONGESTED);
			} 
			
			memset(newMsg,0,sizeof(message_t));
			
			qe->msg = m;
			qe->client = 0xff;
			qe->retries = MAX_RETRIES;
			
			
			if(call SendQueue.enqueue(qe) == SUCCESS) {
				if(!call RetxmitTimer.isRunning()) {
					post sendTask();
				}
				return newMsg;
			}
			else {
				call MessagePool.put(newMsg);
				call QEntryPool.put(qe);
			}
		}
		return m;
	}
	
	
	
	
	
	event message_t*
	SubReceive.receive(message_t* msg,void* payload ,uint8_t len) {
		collection_id_t collectid;
		bool dupicate = FALSE;
		fe_queue_entry_t* qe;
		uint8_t thl;
		uint8_t i;
		
		collectid = getHeader(msg)->type;
		
		thl = getHeader(msg)->thl;
		thl++;
		getHeader(msg)->thl = thl;
		
		if(len > call SubSend.maxPayloadLength()) {
			return msg;
		}
		if(call SentCache.lookup(msg)) {
			return msg;     // looking in the cache ,if we have sent the Pkt ,return
		}
		
		if(call SendQueue.size() > 0){  // looking in the SendQueue
			for(i = call SendQueue.size();i > 0; i-- ){
				qe = call SendQueue.element(i-1);
				if(call CtpPacket.matchInstance(qe->msg ,msg)){
					dupicate = TRUE;
					break;
				}
			}
		}
		if(dupicate) {
			return msg;
		}
		else if(call RootControl.isRoot()) {
			return signal Receive.receive[collectid](msg,
										call Packet.getPayload(msg, call Packet.payloadLength(msg)), 
										call Packet.payloadLength(msg));
		}
		else if(!signal Intercept.forward[collectid](msg,
											call Packet.getPayload(msg, call Packet.payloadLength(msg)),
											call Packet.payloadLength(msg))) {
			return msg;
		}
		else {
			return forward(msg);
		}
		
	}
	
	event message_t* 
	SubSnoop.receive(message_t* msg, void *payload, uint8_t len) {
		
		//if(call CtpPacket.option(msg,CTP_OPT_PULL)){
			//call CtpInfo.triggerRouteUpdate();
		//}
		return signal Snoop.receive[call CtpPacket.getType(msg)](msg,
							payload + sizeof(ctp_data_header_t),
							len - sizeof(ctp_data_header_t));
	}
	
	
	event void RetxmitTimer.fired(){
		clearState(SENDING);
		post sendTask();
		//pr("Retxmit\n");
	}
	
	command bool CtpCongestion.isCongested() {
		return FALSE;
	}
	
	command void CtpCongestion.setClientCongested(bool congested) {
		
	}
	
	//event void LinkEstimator.evicted(am_addr_t neighbor) {
	//}
	
	command void Packet.clear(message_t* msg) {
		call SubPacket.clear(msg);
	}
	
	command uint8_t Packet.payloadLength(message_t* msg) {
		return call SubPacket.payloadLength(msg) - sizeof(ctp_data_header_t);
	}
	
	command void Packet.setPayloadLength(message_t* msg,uint8_t len) {
		return call SubPacket.setPayloadLength(msg,len + sizeof(ctp_data_header_t));
	}
	
	command uint8_t Packet.maxPayloadLength() {
		return call SubPacket.maxPayloadLength() - sizeof(ctp_data_header_t);
	}
	
	
	command void* Packet.getPayload(message_t* msg, uint8_t len) {
		uint8_t* payload = call SubPacket.getPayload(msg,len + sizeof(ctp_data_header_t));
		if(payload != NULL) {
			payload += sizeof(ctp_data_header_t);
		}
		return payload;
	}
	
	//collectionPacket ADT commands
	command am_addr_t CollectionPacket.getOrigin(message_t* msg) {
		return getHeader(msg)->origin;
	}
	command collection_id_t CollectionPacket.getType(message_t* msg){
		return getHeader(msg)->type;
	}
	command uint8_t	CollectionPacket.getSequenceNumber(message_t* msg){
		return getHeader(msg)->originSeqNo;
	}
	command void CollectionPacket.setOrigin(message_t* msg,am_addr_t addr){
		getHeader(msg)->origin = addr;
	}
	command void CollectionPacket.setType(message_t* msg,collection_id_t id) {
		getHeader(msg)->type = id;
	}
	command void CollectionPacket.setSequenceNumber(message_t* msg,uint8_t _seqno) {
		getHeader(msg)->originSeqNo = _seqno;
	}
	
	// CtpPacket ADT commands
	command uint8_t CtpPacket.getType(message_t* msg) {
		return getHeader(msg)->type;
	}
	command am_addr_t CtpPacket.getOrigin(message_t* msg) {
		return getHeader(msg)->origin;
	}
	//command uint16_t CtpPacket.getExt(message_t* msg) {
		//return getHeader(msg)->etx;
	//}
	command uint8_t CtpPacket.getSequenceNumber(message_t* msg){
		return getHeader(msg)->originSeqNo;
	}	
	command uint8_t CtpPacket.getThl(message_t* msg) {
		return getHeader(msg)->thl;
	}
	command void CtpPacket.setThl(message_t* msg, uint8_t thl) {
		getHeader(msg)->thl = thl;
	}
	command void CtpPacket.setOrigin(message_t* msg, am_addr_t addr) {
		getHeader(msg)->origin = addr;
	}
	command void CtpPacket.setType(message_t* msg, uint8_t id) {
		getHeader(msg)->type = id;
	}
	//command void CtpPacket.setEtx(message_t* msg, uint16_t e) {getHeader(msg)->etx = e;}
	command void CtpPacket.setSequenceNumber(message_t* msg, uint8_t _seqno) {
		getHeader(msg)->originSeqNo = _seqno;
	}
	command bool CtpPacket.option(message_t* msg, ctp_options_t opt) {
		return ((getHeader(msg)->options & opt) == opt) ? TRUE : FALSE;
	}
	command void CtpPacket.setOption(message_t* msg, ctp_options_t opt) {
		getHeader(msg)->options |= opt;
	}
	command void CtpPacket.clearOption(message_t* msg, ctp_options_t opt) {
		getHeader(msg)->options &= ~opt;
	}
	
	
	// 
	command bool CtpPacket.matchInstance(message_t* m1,message_t* m2) {
		return (call CtpPacket.getOrigin(m1) == call CtpPacket.getOrigin(m2) &&
				call CtpPacket.getSequenceNumber(m1) == call CtpPacket.getSequenceNumber(m2) &&
#ifndef DISABLE_THL_CHECK
				call CtpPacket.getThl(m1) == call CtpPacket.getThl(m2) &&
#endif			
				call CtpPacket.getType(m1) == call CtpPacket.getType(m2));
	}
	
	command bool CtpPacket.matchPacket(message_t* m1,message_t* m2) {
		return (call CtpPacket.getOrigin(m1) == call CtpPacket.getOrigin(m2) &&
				call CtpPacket.getSequenceNumber(m1) == call CtpPacket.getSequenceNumber(m2) &&
				call CtpPacket.getType(m1) == call CtpPacket.getType(m2));
	}
	
	/*****Defaults*****/
	default event void Send.sendDone[uint8_t client](message_t *msg, error_t error) {
	}
	
	default event bool
	Intercept.forward[collection_id_t collectid](message_t* msg,void *payload,uint8_t len){
		return TRUE;
	}
	
	default event message_t* 
	Receive.receive[collection_id_t collectid](message_t *msg,void *payload,uint8_t len ){
		return msg;
	}
	
	default event message_t *
	Snoop.receive[collection_id_t collectid](message_t *msg, void *payload,uint8_t len) {
		return msg;
	}

	default command collection_id_t CollectionId.fetch[uint8_t client]() {
		return 0;
	}
	
	//command uint8_t CtpFwdInfo.getQueueLength() {
		//return call SendQueue.size();
	//}
	//default event void CtpFwdInfo.ctpRetrans(message_t *msg, void *payload, uint8_t length) {
		
	//}
	
}